#include "CheckBoard.h"
#include <SensorUtility/sensorutility_global.h>
#include <glm/glm.hpp>

CheckBoard::CheckBoard (int rows, int cols, float size) :
		_corners (rows, cols, CV_32FC3)
{
	for (int v = 0; v < rows; ++v)
		for (int u = 0; u < cols; ++u)
			_corners.at< cv::Vec3f >(v, u) =
					cv::Vec3f ((float)u * size, (rows - 1 - (float)v) * size, 0.0f);
}

CheckBoard::~CheckBoard ()
{

}

const cv::Mat& CheckBoard::getCorners () const
{
	return _corners;
}

cv::Mat CheckBoard::findCheckBoard (const QImage& img, bool& outisFound)
{
	outisFound = false;
	if (img.isNull ())
		return cv::Mat ();
	cv::Mat image (img.height (), img.width (),
			qImageFormat2CvImageType (img.format ()), (void*)img.bits ());

	cv::Mat CornersFound;
	outisFound = cv::findChessboardCorners (image,
			cv::Size (_corners.cols, _corners.rows), CornersFound);
	int cornerCount = _corners.rows * _corners.cols;
// 	if (outisFound) {
// 		// 		cvFindCornerSubPix (image, CornersFound, cornerCount,
// 		// 				cvSize (5, 5), cvSize (1, 1),
// 		// 				cvTermCriteria (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 10, 0.02));
// 		if (glm::length (glm::vec2 (CornersFound[0].x, CornersFound[0].y)) >
// 				glm::length (glm::vec2 (CornersFound[cornerCount - 1].x,
// 						CornersFound[cornerCount - 1].y))) {
// 			for (int i = 0; i < cornerCount / 2; ++i) {
// 				cv::Point2f temp = CornersFound[i];
// 				CornersFound[i] = CornersFound[cornerCount - 1 - i];
// 				CornersFound[cornerCount - 1 - i] = temp;
// 			}
// 		}
// 	}
	return CornersFound;
}
